/*
#include <Motion.h>
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-11-28     Administrator       the first version
 */


#include "ActionQueue.h"
#include <rtthread.h>
#include <stdlib.h>
#include <iostream>
#include <cstring>
#include "ulog.h"

#include "GlobalVar.h"

#ifdef MOTOR_FUNC
#include "MotionManage.h"
#include "Motion.h"
#endif

//#include "comm.h"

ActionQueue ActQueue[50];


/*
* Function name: ActionQueue()
* Input argv:
*      --
* Output argv:
*      --
* Return:
       true: success
       false: fail
* Function Description:  the constructor
* Version: 1.0
* Be careful:
*/

ActionQueue::ActionQueue()
{
    //No = _No;
}

/*
* Function name: ~ActionQueue()
* Input argv:
*      --
* Output argv:
*      --
* Return:
       true: success
       false: fail
* Function Description:  the destructor
* Version: 1.0
* Be careful:
*/
ActionQueue::~ActionQueue()
{

}

/*
* Function name: ~ActionQueue()
* Input argv:
*      --
* Output argv:
*      --
* Return:
       true: success
       false: fail
* Function Description:  the destructor
* Version: 1.0
* Be careful:
*/
struct Action_Queue* ActionQueue::CreatActionQueue(void *parameter)
{
    Action_Queue* headNode;
    headNode = (struct Action_Queue*) malloc(sizeof(struct Action_Queue));
    //headNode->devType = L->devType;
    headNode->next = NULL;
    LOG_I("Create head node successed.");
    return headNode;
}

struct Action_Queue* ActionQueue::TailInsertActionQueue(Action_Queue temp,Action_Queue* tail)
{
    if(tail->next != NULL)
    {
        LOG_E("Node tail is not a tail.");
        return tail;
    }
    Action_Queue *s;
    s = (struct Action_Queue*) malloc(sizeof(struct Action_Queue));
    s->devType = temp.devType;
    s->devNo = temp.devNo;
    s->actMode = temp.actMode;
    memcpy(&(s->actInfo),&(temp.actInfo),sizeof(temp.actInfo));
    tail->next = s;
    s->next = NULL;
    LOG_I("Add tail node successed.");
    return s;
}

struct Action_Queue* ActionQueue::DeleteHeadNode(Action_Queue* headNode)
{
    headNode = headNode->next;
    return headNode;
}

void ActionQueue::CreatActionQueueTask(Action action)
{
    //等待一个信号量

    //CreatActionQueue
}

void ActionQueue::AddActionQueueTask(Action action)
{
    //等待一个信号量
    //CreatActionQueue
}

/*  如果包含电机功能，则执行这一部分 */
#ifdef MOTOR_FUNC


void ActionQueue::AddTaskToMotor(rt_uint8_t devNo,rt_uint8_t Mode)
{
    switch(headNode->actMode)
    {
        case MODE_POS:
            StartPosMode(headNode->devNo,headNode->actInfo);
            break;
        case MODE_AD_POS:
            //StartAdPosMode();
            break;
    }
}

void MotorDirverStartUp(rt_uint8_t dirveNnum,ACT_INFO ActInfo)
{
    /* 获取一下模块位置 */
    rt_uint32_t tagetPos = DummyMotorPtr[dirveNnum]->GetPosInfo(ActInfo.posNo);
    /* 设置目标位置 */
    tmc_driver[dirveNnum]->SetTargetPos(tagetPos);
    /* 发送一个信号量启动电机 */
    rt_sem_release(sem_motor[dirveNnum].RunStart);
}

void ActionQueue::StartPosMode(rt_uint8_t devNo,ACT_INFO ActInfo)
{
    /* Retrieve the hardware link for the device */
    rt_uint8_t dirver_num = MotorDiverNum(devNo);
    /* 启动对应的驱动器 */
    MotorDirverStartUp(dirver_num,ActInfo);
}


/*  如果包含电机功能，则执行这一部分 */
#endif



void WaitMotorActComplete(rt_uint8_t DevNo)
{
    if(MyBoardMotor(DevNo) == true)
    {
        rt_uint8_t dirver_num = MotorDiverNum(DevNo);
        /* 等待电机驱动完成信号 */
        if(rt_sem_take(sem_motor[dirver_num].RunFinished, MOTOR_TIME_OUT) == -RT_ETIMEOUT)
        {
            /* 器件超时报错 */
            //发送一个错误邮件或者信号量
            LOG_E("Board %d Motor Dirver %D DummyMotor %d is Timeout.",machine_info.addr,dirver_num,DevNo);
        }
        else
        {
        }
    }
    else
    {
        /* 等待CAN 返回器件完成信号 */
        //rt_sem_take(can_sem,time);
    }
}

void ActionQueue::NextStep()
{
    /* 删除头节点 将下一个节点置为头节点  */
    headNode = DeleteHeadNode(headNode);
};


void ActionQueue::ActOneStep()
{
    switch(headNode->devType)
    {
        case MOTOR:
            if(MyBoardMotor(headNode->devNo) == true)
            {
                /*  如果包含电机功能，则执行这一部分 */
                #ifdef MOTOR_FUNC
                    /*  添加一条电机任务 */
                    AddTaskToMotor(headNode->devNo,headNode->actMode);
                #else
                    LOG_E("Board have no motor Dirver.");
                #endif
            }
            else
            {
                //SendMsg();
            }
            /*  等待完成 */
            WaitMotorActComplete(headNode->devNo);
            //打印当前时序
            break;
        case PUMP:
            break;
    }
}

void ActionQueue::Process(void *parameter)
{
    //等待一个信号量开启进程
    //CreatActionQueue

    //示例  释放一个电机1运动信号量
//    rt_thread_mdelay(3000);
//    LOG_D(" m1 start...");
//    rt_sem_release(sem_motor[1].RunStart);
//    rt_thread_mdelay(20000);
    //示例结束
    headNode = CreatActionQueue(NULL);
    //初始化测试任务
    headNode->devType = MOTOR;
    headNode->devNo = 1;
    headNode->actMode = MODE_POS;
    headNode->actInfo.posNo = 1;

    Mode = MODE_AUTO;

    Action_Queue temp;
    Action_Queue *tail;

    temp.devType = MOTOR;
    temp.devNo = 2;
    temp.actMode = MODE_POS;
    temp.actInfo.posNo = 1;

    tail = TailInsertActionQueue(temp,headNode);

    temp.devType = MOTOR;
    temp.devNo = 4;
    temp.actMode = MODE_POS;
    temp.actInfo.posNo = 1;

    tail = TailInsertActionQueue(temp,tail);

    temp.devType = MOTOR;
    temp.devNo = 3;
    temp.actMode = MODE_POS;
    temp.actInfo.posNo = 1;

    tail = TailInsertActionQueue(temp,tail);

    Total = 4;
    //初始化测试任务结束

    switch(Mode)
    {
        case MODE_SLAVE:
            break;

        case MODE_AUTO:
            break;
    }


    while(1)
    {
        /*  执行一步动作  */
        ActOneStep();
        Total--;
        /*  下一任务 */
        if(Total != 0)
        {
            NextStep();
        }
        else //if()
        {
            return;
        }
    }

}


void Act_Q1_Task(void *parameter)
{
    while(1)
    {
        ActQueue[1].Process(parameter);
    }
}





